#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <chrono>
#include <vector>
#include <string>
#include <GeographicLib/LocalCartesian.hpp>
#include <GeographicLib/Constants.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <yolo_detect_message/msg/geolocation.hpp>
using namespace std::chrono_literals;

class GroundtruthNode : public rclcpp::Node {
public:
    GroundtruthNode()
    : Node("getlocalpos_node"), 
      tf_buffer_(std::make_shared<tf2_ros::Buffer>(this->get_clock())),
      tf_listener_(*tf_buffer_) {
        initialize_parameters();

        global_orgin_initialized = false;
        multi_pose_pub_.resize(vehicle_num_);
        // multi_speed_pub_.resize(vehicle_num_);
        multi_local_pose_.resize(vehicle_num_);
        // multi_speed_.resize(vehicle_num_);

        global_pose_gps.resize(vehicle_num_);

        global_pose_subscribers_.resize(0);

        is_first_.resize(vehicle_num_, false);

        // 创建发布器和订阅器
        for (int i = 0; i < vehicle_num_; ++i) {
            create_publishers(i);
            create_subscribers(i);
        }
        global_origin_location_pub_ = this->create_publisher<yolo_detect_message::msg::Geolocation>("/uav_swarms/global_init_positon_gps",100);
        timer_ = this->create_wall_timer(50ms, std::bind(&GroundtruthNode::timer_loop, this));
        // RCLCPP_INFO(this->get_logger(), "Get %s groundtruth pose and speed", vehicle_type_.c_str());
    }

    void initialize_parameters() {
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<int>("vehicle_num", 3);
        this->declare_parameter<double>("latitude_num", 47.6146478);
        this->declare_parameter<double>("longitude_num", -122.1489182);
        this->declare_parameter<double>("altitude_num", 700.4873627557417);
        
        this->declare_parameter<bool>("is_origin_autoinitial",true);  
        vehicle_type_ = this->get_parameter("vehicle_type").as_string();
        namespaces = this->get_parameter("uav_namespace").as_string();


        vehicle_num_ = this->get_parameter("vehicle_num").as_int();  // 确保 vehicle_num 是整数
        reference_latitude_ = this->get_parameter("latitude_num").as_double(); 
        reference_longitude_ = this->get_parameter("longitude_num").as_double(); 
        reference_altitude_ = this->get_parameter("altitude_num").as_double(); 
        is_initial_origin = this->get_parameter("is_origin_autoinitial").as_bool();

        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type_.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", vehicle_num_);
        RCLCPP_INFO(this->get_logger(), "reference_latitude_: %f", reference_latitude_);
        RCLCPP_INFO(this->get_logger(), "reference_longitude_: %f", reference_longitude_);
        RCLCPP_INFO(this->get_logger(), "reference_altitude_: %f", reference_altitude_);
        
    }

private:
    void create_publishers(int index) {
        multi_pose_pub_[index] = this->create_publisher<geometry_msgs::msg::PoseStamped>(
            vehicle_type_ + "_" + std::to_string(index) + "/vision_pose/pose", 10);
        RCLCPP_INFO(this->get_logger(), "Publisher created for %s_%d", vehicle_type_.c_str(), index);
    }

    void create_subscribers(int index) {
        // 使用QoS配置提高数据的可靠性

        auto global_pose_callback = [this, index](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
            process_global_msg(msg, index);
        };


        global_pose_subscribers_.emplace_back(this->create_subscription<sensor_msgs::msg::NavSatFix>(
            (vehicle_type_ + "_" + std::to_string(index) + "/global_position/global").c_str(), rclcpp::QoS(10).best_effort(), global_pose_callback));
    }

    void process_pose_msg(const geometry_msgs::msg::PoseStamped::SharedPtr msg, int index) {
        
    }

    void process_velocity_msg(const geometry_msgs::msg::TwistStamped::SharedPtr msg, int index) {
       
    }


    void process_global_msg(const sensor_msgs::msg::NavSatFix::SharedPtr msg, int index) {
        if ((index == 0 && is_initial_origin == false) && global_orgin_initialized==false)
        {
            reference_latitude_ = msg->latitude;
            reference_altitude_ = msg->altitude;
            reference_longitude_ = msg->longitude;
            global_orgin_initialized = true;
        }else if(index == 0 && is_initial_origin == true){
            global_orgin_initialized = true;
        } else if(global_orgin_initialized == false){
            return;
        }
       
        global_pose_gps[index] = *msg;
        is_first_[index] = true;

        multi_local_pose_[index] = convert(msg->latitude, msg->longitude, msg->altitude,index);
        multi_pose_pub_[index]->publish(multi_local_pose_[index]);

    }

    void publish_pose_and_speed() {
        for (int i = 0; i < vehicle_num_; ++i) {
            if (is_first_[i]) {
                multi_pose_pub_[i]->publish(multi_local_pose_[i]);
            }
        }
    }

    geometry_msgs::msg::PoseStamped convert(double latitude, double longitude, double altitude,int index) {
        geometry_msgs::msg::PoseStamped pose;
        GeographicLib::LocalCartesian local_cartesian_0;
        local_cartesian_0.Reset(reference_latitude_, reference_longitude_, reference_altitude_);
        local_cartesian_0.Forward(latitude, longitude, altitude, pose.pose.position.x, 
        pose.pose.position.y, 
        pose.pose.position.z);
        return pose;
    }
    void timer_loop(){
        if (global_orgin_initialized)
        {
            yolo_detect_message::msg::Geolocation geo;

            geo.altitude = reference_altitude_;
            geo.latitude = reference_latitude_;
            geo.longtitude = reference_longitude_;

            global_origin_location_pub_->publish(geo); 
            
        }
        
    }
    std::string vehicle_type_;
    int vehicle_num_;
    int uav_id;
    std::vector<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr> multi_pose_pub_;
    std::vector<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr> multi_speed_pub_;
    std::vector<geometry_msgs::msg::PoseStamped> multi_local_pose_;
    std::vector<geometry_msgs::msg::TwistStamped> multi_speed_;
    std::vector<sensor_msgs::msg::NavSatFix> global_pose_gps;
    rclcpp::Publisher<yolo_detect_message::msg::Geolocation>::SharedPtr global_origin_location_pub_;

    std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> global_pose_subscribers_;
    std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> pose_subscribers_;
    std::vector<rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr> velocity_subscribers_;
    std::vector<bool> is_first_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    std::string namespaces;
    double reference_latitude_;
    double reference_longitude_;
    double reference_altitude_;
    bool is_initial_origin = false;

    bool global_orgin_initialized;

};